#include "Common.h"
#include "Math.h"
#include "d3dx9.h"

//------------------------------------------
const Vector3 Vector3::Zero(0, 0, 0);
const Vector3 Vector3::One(1, 1, 1);
const Vector3 Vector3::UnitX(1, 0, 0);
const Vector3 Vector3::UnitY(0, 1, 0);
const Vector3 Vector3::UnitZ(0, 0, 1);
//------------------------------------------
const Vector2 Vector2::Zero(0, 0);
const Vector2 Vector2::One(1, 1);
const Vector2 Vector2::UnitX(1, 0);
const Vector2 Vector2::UnitY(0, 1);
//------------------------------------------
const Quaternion Quaternion::ZERO(0, 0, 0, 0);
const Quaternion Quaternion::Identity(0, 0, 0, 1);
const Rotator Rotator::Identity(0, 0, 0);
//------------------------------------------
const Matrix Matrix::Identity(Vector3::UnitX, Vector3::UnitY, Vector3::UnitZ, Vector3::Zero);
//------------------------------------------
Math GMath;
//------------------------------------------
// Math
Math::Math(void)
{
	for (int i = 0; i < NUM_ANGLES; i++)
		mTable[i] = sin(i * 2.f * PI / NUM_ANGLES);
}
//------------------------------------------
Math::~Math(void)
{
}
//------------------------------------------
Matrix Matrix::Inverse() const
{
	Matrix Result;
	D3DXMatrixInverse((D3DXMATRIX*)&Result, NULL, (const D3DXMATRIX*)this);
	return Result;
}
//------------------------------------------
Quaternion::Quaternion(const Matrix& M)
{
	//const MeReal *const t = (MeReal *) tm;
	FLOAT	s;

	// Check diagonal (trace)
	const FLOAT tr = M.M[0][0] + M.M[1][1] + M.M[2][2];

	if (tr > 0.0f) 
	{
		FLOAT InvS = Math::InvSqrt(tr + 1.f);
		this->W = 0.5f * (1.f / InvS);
		s = 0.5f * InvS;

		this->X = (M.M[1][2] - M.M[2][1]) * s;
		this->Y = (M.M[2][0] - M.M[0][2]) * s;
		this->Z = (M.M[0][1] - M.M[1][0]) * s;
	} 
	else 
	{
		// diagonal is negative
		INT i = 0;

		if (M.M[1][1] > M.M[0][0])
			i = 1;

		if (M.M[2][2] > M.M[i][i])
			i = 2;

		static const INT nxt[3] = { 1, 2, 0 };
		const INT j = nxt[i];
		const INT k = nxt[j];

		s = M.M[i][i] - M.M[j][j] - M.M[k][k] + 1.0f;

		FLOAT InvS = Math::InvSqrt(s);

		FLOAT qt[4];
		qt[i] = 0.5f * (1.f / InvS);

		s = 0.5f * InvS;

		qt[3] = (M.M[j][k] - M.M[k][j]) * s;
		qt[j] = (M.M[i][j] + M.M[j][i]) * s;
		qt[k] = (M.M[i][k] + M.M[k][i]) * s;

		this->X = qt[0];
		this->Y = qt[1];
		this->Z = qt[2];
		this->W = qt[3];
	}
}
//------------------------------------------
void Quaternion::ToRotationMatrix(Matrix3& kRot) const
{
    float fTx  = 2.0f*X;
    float fTy  = 2.0f*Y;
    float fTz  = 2.0f*Z;
    float fTwx = fTx*W;
    float fTwy = fTy*W;
    float fTwz = fTz*W;
    float fTxx = fTx*X;
    float fTxy = fTy*X;
    float fTxz = fTz*X;
    float fTyy = fTy*Y;
    float fTyz = fTz*Y;
    float fTzz = fTz*Z;

    kRot[0][0] = 1.0f-(fTyy+fTzz);
    kRot[0][1] = fTxy-fTwz;
    kRot[0][2] = fTxz+fTwy;
    kRot[1][0] = fTxy+fTwz;
    kRot[1][1] = 1.0f-(fTxx+fTzz);
    kRot[1][2] = fTyz-fTwx;
    kRot[2][0] = fTxz-fTwy;
    kRot[2][1] = fTyz+fTwx;
    kRot[2][2] = 1.0f-(fTxx+fTyy);
}
//------------------------------------------
// Rotator
Rotator::Rotator(const Quaternion& quat)
{
	*this = Matrix::RotationTranslationMatrix(quat, Vector3::Zero).getRotator();
}
//------------------------------------------
Quaternion Rotator::Quat() const
{
	return Quaternion(Matrix::RotationMatrix(*this));
}
//------------------------------------------